/******************************************************************************
 Copyright (c) 2024-2025 Qualcomm Technologies International, Ltd.
 All Rights Reserved.
 Qualcomm Technologies International, Ltd. Confidential and Proprietary.

******************************************************************************/

#include "gatt_ras_server_private.h"

#ifdef INSTALL_CHANNEL_SOUNDING_SUPPORT
#include "gatt_ras_server_debug.h"
#include "gatt_ras_server_access.h"
#include "gatt_ras_server_notify_indicate.h"
#include "csr_bt_cm_prim.h"
#include "csr_bt_cm_lib.h"
#include "csr_bt_gatt_client_util_lib.h"
#include "gatt_ras_server_msg_handler.h"

/* Max buffer size to hold 1 CS subevent */
#define RANGING_BUFFER_SIZE   1000
/* For GATT_RAS_MAX_CONNECTIONS, we maintain buffer each*/
uint8 gRangingDataBuffer[GATT_RAS_MAX_CONNECTIONS][RANGING_BUFFER_SIZE] = {0};
uint16 gRangingDataBufferLen[GATT_RAS_MAX_CONNECTIONS] ={0};

/* From BLUETOOTH CORE SPECIFICATION Atlanta r04 | Vol 6, Part H 
 * Antenna path permutations
 * An antenna path permutation index is used to look up the specific antenna
 * path position order used at each step that contains a CS tone exchange
 *  within a CS step. These lookup tables are defined based on the number of
 * antenna paths being used.
 */

/* A1 = 1 , A2 = 2, A3 = 3, A4= 4 */
static const uint8 antennaPathNum2[2][2] =
{
   {1, 2},
   {2, 1},
};

static const uint8 antennaPathNum3[6][3] = 
{
   {1, 2, 3},
   {2, 1, 3},
   {1, 3, 2},
   {3, 1, 2},
   {3, 2, 1},
   {2, 3, 1},
};

static const uint8 antennaPathNum4[24][4] = 
{
   {1, 2, 3, 4},
   {2, 1, 3, 4},
   {1, 3, 2, 4},
   {3, 1, 2, 4},
   {3, 2, 1, 4},
   {2, 3, 1, 4},
   {1, 2, 4, 3},
   {2, 1, 4, 3},
   {1, 4, 2, 3},
   {4, 1, 2, 3},
   {4, 2, 1, 3},
   {2, 4, 1, 3},
   {1, 4, 3, 2},
   {4, 1 ,3, 2},
   {1, 3, 4, 2},
   {3, 1, 4, 2},
   {3, 4, 1, 2},
   {4, 3, 1, 2},
   {4, 2, 3, 1},
   {2, 4, 3, 1},
   {4, 3, 2, 1},
   {3, 4, 2, 1},
   {3, 2, 4, 1},
   {2, 3, 4, 1},
};

#define ANTENNA_PATH_PERMUTATION_INDEX_AP4_MAX_VALUE   23
#define ANTENNA_PATH_PERMUTATION_INDEX_AP3_MAX_VALUE   5
#define ANTENNA_PATH_PERMUTATION_INDEX_AP2_MAX_VALUE   1

#define ANTENNA_PATH_AP1   1
#define ANTENNA_PATH_AP2   2
#define ANTENNA_PATH_AP3   3
#define ANTENNA_PATH_AP4   4

#if 0
static void displayHciStepData(uint8 *data, uint8 length)
{
    uint16 length1;
    GATT_RAS_SERVER_INFO(" Steps total length: %x ", length);

    GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[0],data[1],data[2],data[3], data[4], data[5], data[6],data[7] ,data[8] ,data[9], data[10],data[11],data[12],data[13], data[14], data[15]);

    if (length > 16)
    {
        length1 = 16;
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+0],data[length1+1],data[length1+2],data[length1+3], data[length1+4], data[length1+5], data[length1+6],data[length1+7] ,data[length1+8] ,data[length1+9], data[length1+10],data[length1+11],data[length1+12],data[length1+13], data[length1+14], data[length1+15]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+16],data[length1+17] ,data[length1+18] ,data[length1+19], data[length1+20],data[length1+21],data[length1+22],data[length1+23], data[length1+24], data[length1+25], data[length1+26],data[length1+27] ,data[length1+28] ,data[length1+29],data[length1+30],data[length1+31]);
    }

    if (length > 48)
    {
        length1 = 48;
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+0],data[length1+1],data[length1+2],data[length1+3], data[length1+4], data[length1+5], data[length1+6],data[length1+7] ,data[length1+8] ,data[length1+9], data[length1+10],data[length1+11],data[length1+12],data[length1+13], data[length1+14], data[length1+15]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+16],data[length1+17] ,data[length1+18] ,data[length1+19], data[length1+20],data[length1+21],data[length1+22],data[length1+23], data[length1+24], data[length1+25], data[length1+26],data[length1+27] ,data[length1+28] ,data[length1+29],data[length1+30],data[length1+31]);
    }

    if (length > 80)
    {
        length1 = 80;
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+0],data[length1+1],data[length1+2],data[length1+3], data[length1+4], data[length1+5], data[length1+6],data[length1+7] ,data[length1+8] ,data[length1+9], data[length1+10],data[length1+11],data[length1+12],data[length1+13], data[length1+14], data[length1+15]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+16],data[length1+17] ,data[length1+18] ,data[length1+19], data[length1+20],data[length1+21],data[length1+22],data[length1+23], data[length1+24], data[length1+25], data[length1+26],data[length1+27] ,data[length1+28] ,data[length1+29],data[length1+30],data[length1+31]);
    }

    if (length > 112)
    {
        length1 = 112;
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+0],data[length1+1],data[length1+2],data[length1+3], data[length1+4], data[length1+5], data[length1+6],data[length1+7] ,data[length1+8] ,data[length1+9], data[length1+10],data[length1+11],data[length1+12],data[length1+13], data[length1+14], data[length1+15]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+16],data[length1+17] ,data[length1+18] ,data[length1+19], data[length1+20],data[length1+21],data[length1+22],data[length1+23], data[length1+24], data[length1+25], data[length1+26],data[length1+27] ,data[length1+28] ,data[length1+29],data[length1+30],data[length1+31]);
    }

    if (length > 144)
    {
        length1 = 144;
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+0],data[length1+1],data[length1+2],data[length1+3], data[length1+4], data[length1+5], data[length1+6],data[length1+7] ,data[length1+8] ,data[length1+9], data[length1+10],data[length1+11],data[length1+12],data[length1+13], data[length1+14], data[length1+15]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+16],data[length1+17] ,data[length1+18] ,data[length1+19], data[length1+20],data[length1+21],data[length1+22],data[length1+23], data[length1+24], data[length1+25], data[length1+26],data[length1+27] ,data[length1+28] ,data[length1+29],data[length1+30],data[length1+31]);
    }

    if (length > 176)
    {
        length1 = 176;
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+0],data[length1+1],data[length1+2],data[length1+3], data[length1+4], data[length1+5], data[length1+6],data[length1+7] ,data[length1+8] ,data[length1+9], data[length1+10],data[length1+11],data[length1+12],data[length1+13], data[length1+14], data[length1+15]);
    }

    if (length > 192)
    {
        length1 = 192;
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+0],data[length1+1],data[length1+2],data[length1+3], data[length1+4], data[length1+5], data[length1+6],data[length1+7] ,data[length1+8] ,data[length1+9], data[length1+10],data[length1+11],data[length1+12],data[length1+13], data[length1+14], data[length1+15]);
    }

    if (length > 208)
    {
        length1 = 208;
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+0],data[length1+1],data[length1+2],data[length1+3], data[length1+4], data[length1+5], data[length1+6],data[length1+7] ,data[length1+8] ,data[length1+9], data[length1+10],data[length1+11],data[length1+12],data[length1+13], data[length1+14], data[length1+15]);
    }

    if (length > 224)
    {
        length1 = 224;
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+0],data[length1+1],data[length1+2],data[length1+3], data[length1+4], data[length1+5], data[length1+6],data[length1+7] ,data[length1+8] ,data[length1+9], data[length1+10],data[length1+11],data[length1+12],data[length1+13], data[length1+14], data[length1+15]);
    }

}

static void displayRasSegmentData(uint8 *data, uint8 length, bool lastSegment)
{
    CSR_UNUSED(lastSegment);
    uint16 length1;
    GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[0],data[1],data[2],data[3], data[4], data[5], data[6],data[7] ,data[8] ,data[9], data[10],data[11],data[12],data[13], data[14], data[15]);
    GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[16],data[17] ,data[18] ,data[19], data[20],data[21],data[22],data[23], data[24], data[25], data[26],data[27] ,data[28] ,data[29],data[30],data[31]);
    GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[32],data[33], data[34], data[35], data[36],data[37] ,data[38] ,data[39], data[40],data[41],data[42],data[43], data[44], data[45], data[46],data[47]); 
    GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[48] ,data[49],data[50],data[51],data[52],data[53], data[54], data[55], data[56],data[57] ,data[58] ,data[59], data[60],data[61],data[62],data[63]);
    GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[64], data[65], data[66],data[67] ,data[68] ,data[69],data[70],data[71],data[72],data[73], data[74], data[75], data[76],data[77] ,data[78] ,data[79]);
    GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[80],data[81],data[82],data[83], data[84], data[85], data[86],data[87] ,data[88] ,data[89], data[90],data[91],data[92],data[93], data[94], data[95]);
    GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x",  data[96],data[97] ,data[98] ,data[99],data[100],data[101],data[102],data[103], data[104], data[105], data[106],data[107] ,data[108] ,data[109], data[110],data[111]);
    GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x",data[112], data[113], data[114], data[115],data[116] ,data[117] ,data[118], data[119], data[120]);

    if (length > 121)
    {
        length1 = 121;
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+0],data[length1+1],data[length1+2],data[length1+3], data[length1+4], data[length1+5], data[length1+6],data[length1+7] ,data[length1+8] ,data[length1+9], data[length1+10],data[length1+11],data[length1+12],data[length1+13], data[length1+14], data[length1+15]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+16],data[length1+17] ,data[length1+18] ,data[length1+19], data[length1+20],data[length1+21],data[length1+22],data[length1+23], data[length1+24], data[length1+25], data[length1+26],data[length1+27] ,data[length1+28] ,data[length1+29],data[length1+30],data[length1+31]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+32],data[length1+33], data[length1+34], data[length1+35], data[length1+36],data[length1+37] ,data[length1+38] ,data[length1+39], data[length1+40],data[length1+41],data[length1+42],data[length1+43], data[length1+44], data[length1+45], data[length1+46],data[length1+47]); 
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+48] ,data[length1+49],data[length1+50],data[length1+51],data[length1+52],data[length1+53], data[length1+54], data[length1+55], data[length1+56],data[length1+57] ,data[length1+58] ,data[length1+59], data[length1+60],data[length1+61],data[length1+62],data[length1+63]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+64], data[length1+65], data[length1+66],data[length1+67] ,data[length1+68] ,data[length1+69],data[length1+70],data[length1+71],data[length1+72],data[length1+73], data[length1+74], data[length1+75], data[length1+76],data[length1+77] ,data[length1+78] ,data[length1+79]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+80],data[length1+81],data[length1+82],data[length1+83], data[length1+84], data[length1+85], data[length1+86],data[length1+87] ,data[length1+88] ,data[length1+89], data[length1+90],data[length1+91],data[length1+92],data[length1+93], data[length1+94], data[length1+95]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x",  data[length1+96],data[length1+97] ,data[length1+98] ,data[length1+99],data[length1+100],data[length1+101],data[length1+102],data[length1+103], data[length1+104], data[length1+105], data[length1+106],data[length1+107] ,data[length1+108] ,data[length1+109], data[length1+110],data[length1+111]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x",data[length1+112], data[length1+113], data[length1+114], data[length1+115],data[length1+116] ,data[length1+117] ,data[length1+118], data[length1+119], data[length1+120]);
    }

    if (length > 241)
    {
        length1 = 241;
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+0],data[length1+1],data[length1+2],data[length1+3], data[length1+4], data[length1+5], data[length1+6],data[length1+7] ,data[length1+8] ,data[length1+9], data[length1+10],data[length1+11],data[length1+12],data[length1+13], data[length1+14], data[length1+15]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+16],data[length1+17] ,data[length1+18] ,data[length1+19], data[length1+20],data[length1+21],data[length1+22],data[length1+23], data[length1+24], data[length1+25], data[length1+26],data[length1+27] ,data[length1+28] ,data[length1+29],data[length1+30],data[length1+31]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+32],data[length1+33], data[length1+34], data[length1+35], data[length1+36],data[length1+37] ,data[length1+38] ,data[length1+39], data[length1+40],data[length1+41],data[length1+42],data[length1+43], data[length1+44], data[length1+45], data[length1+46],data[length1+47]); 
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+48] ,data[length1+49],data[length1+50],data[length1+51],data[length1+52],data[length1+53], data[length1+54], data[length1+55], data[length1+56],data[length1+57] ,data[length1+58] ,data[length1+59], data[length1+60],data[length1+61],data[length1+62],data[length1+63]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+64], data[length1+65], data[length1+66],data[length1+67] ,data[length1+68] ,data[length1+69],data[length1+70],data[length1+71],data[length1+72],data[length1+73], data[length1+74], data[length1+75], data[length1+76],data[length1+77] ,data[length1+78] ,data[length1+79]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x", data[length1+80],data[length1+81],data[length1+82],data[length1+83], data[length1+84], data[length1+85], data[length1+86],data[length1+87] ,data[length1+88] ,data[length1+89], data[length1+90],data[length1+91],data[length1+92],data[length1+93], data[length1+94], data[length1+95]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x",  data[length1+96],data[length1+97] ,data[length1+98] ,data[length1+99],data[length1+100],data[length1+101],data[length1+102],data[length1+103], data[length1+104], data[length1+105], data[length1+106],data[length1+107] ,data[length1+108] ,data[length1+109], data[length1+110],data[length1+111]);
        GATT_RAS_SERVER_INFO(" %x %x %x %x %x %x %x %x",data[length1+112], data[length1+113], data[length1+114], data[length1+115],data[length1+116] ,data[length1+117] ,data[length1+118], data[length1+119]);
    }

}
#endif

static void *rasPmemRealloc(void *ptr, CsrSize oldSize, CsrSize newSize)
{
    void *new;

    new = CsrPmemAlloc(newSize);

    if (ptr)
    {
        if (oldSize < newSize)
        {
            CsrMemCpy(new, ptr, oldSize);
        }
        else
        {
            CsrMemCpy(new, ptr, newSize);
        }
        CsrPmemFree(ptr);
    }
    return new;
}

static void addRangingDataElementNew(GRASS_T *rasSrver, uint16 client_index,
                                     ras_ranging_data_list ** list,
                                     uint8 *buffer, uint16 bufLen, 
                                     bool firstEvent,
                                     uint16 mtu, uint8 procedureDoneStatus)
{
    ras_ranging_data_list *newListElement = NULL;
    ras_ranging_data_list *temp;
    uint16 size, i;
/* Reduce by 4 octet because of one octet header and 3 octet so remote should not invoke blob request */
    uint16 maxSegmentSizeWithoutHeader = mtu - 4;
    bool addSegment = FALSE;
    uint16 rollingCounter = rasSrver->data.connectedClients[client_index].rollingCounter;
    bool lastEvent = FALSE;

    GATT_RAS_SERVER_DEBUG("addRangingDataElementNew");

    newListElement = (ras_ranging_data_list*)CsrPmemZalloc(sizeof(ras_ranging_data_list));

    if (*list == NULL)
    {
        GATT_RAS_SERVER_INFO("list is NULL, this is first segment we are abt to create");
        newListElement->next = *list;
        firstEvent = TRUE;
    }

    if (bufLen < maxSegmentSizeWithoutHeader)
    {
       size = bufLen + 1 ; /* +1 for segment header */
       newListElement->partial = TRUE;
       if (procedureDoneStatus == ALL_RESULTS_COMPLETE_FOR_CS_PROCEDURE)
       {
           lastEvent = TRUE;
       }
    }
    else
    {
       size = maxSegmentSizeWithoutHeader + 1 ; /* +1 for segment header */
       newListElement->partial = FALSE;
       addSegment = TRUE;
       lastEvent = FALSE;
    }

    newListElement->consumed = TRUE;
    newListElement->transmitted = FALSE;

    newListElement->data = (uint8*)CsrPmemZalloc(size);

    rasSrver->data.connectedClients[client_index].numOfSegments = rasSrver->data.connectedClients[client_index].numOfSegments + 1;

    if (newListElement->data == NULL)
        GATT_RAS_SERVER_PANIC("No Memory\n");

    if (firstEvent && lastEvent)
    {
        newListElement->data[0] = FIRST_SEGMENT_TRUE | LAST_SEGMENT_TRUE;
    }
    else if (firstEvent && !lastEvent)
    {
        newListElement->data[0] = FIRST_SEGMENT_TRUE | LAST_SEGMENT_FALSE;
    }
    else if (!firstEvent && !lastEvent)
    {
         newListElement->data[0] = FIRST_SEGMENT_FALSE | LAST_SEGMENT_FALSE;
    }
    else if (lastEvent)
    {
         newListElement->data[0] = FIRST_SEGMENT_FALSE | LAST_SEGMENT_TRUE;
    }

    newListElement->data[0] |= (rollingCounter << 2);
    newListElement->rollingCounter = rasSrver->data.connectedClients[client_index].rollingCounter;

    rasSrver->data.connectedClients[client_index].rollingCounter++;

    if (rasSrver->data.connectedClients[client_index].rollingCounter == ROLLING_COUNTER_MAX)
        rasSrver->data.connectedClients[client_index].rollingCounter = 0;

    for (i = 1 ; i < size; i++)
        newListElement->data[i] = buffer[i-1];

    newListElement->dataLen = size;

    GATT_RAS_SERVER_INFO("dataLen =%d, consumed =%d, partial =%d", newListElement->dataLen,
         newListElement->consumed, newListElement->partial);

    if (*list == NULL)
    {
        GATT_RAS_SERVER_INFO("list is NULL");
        *list = newListElement;
    }
    else
    {
        temp = *list;
        while(temp->next != NULL)
        temp = temp->next;
        temp->next = newListElement;
    }

    if (addSegment)
    {
        addRangingDataElementNew(rasSrver,
                                 client_index,
                                 &(rasSrver->data.connectedClients[client_index].rangingDataList),
                                 &buffer[maxSegmentSizeWithoutHeader],
                                 (bufLen- (maxSegmentSizeWithoutHeader)),
                                 FALSE,
                                 mtu,
                                 procedureDoneStatus);
    }

}

static void addRangingDataExistingElement(GRASS_T *rasSrver,
                                          uint16 client_index,
                                          ras_ranging_data_list ** list,
                                          uint8 *buffer, uint16 bufLen, 
                                          bool firstEvent,
                                          uint16 mtu, uint8 procedureDoneStatus)
{
    ras_ranging_data_list *iterElement = *list;
    uint16 size, i, j = 0;
    uint16 offset;
/* since this is existing element, we are not adding segment header for it. hence the value is 3 */
    uint16 maxSegmentSizeWithHeader = mtu - 3;

    GATT_RAS_SERVER_INFO("addRangingDataExistingElement");

    while(iterElement != NULL)
    {
        if(iterElement->consumed == TRUE && iterElement->partial == TRUE)
            break;

        iterElement = iterElement->next;
    }

    if (bufLen < (maxSegmentSizeWithHeader - iterElement->dataLen))
    {
        /* data is less and can fit in existing element */
        size = iterElement->dataLen + bufLen;

        iterElement->data = (uint8 *) rasPmemRealloc(iterElement->data, iterElement->dataLen, size);

        for (i = iterElement->dataLen ; i< size; i++, j++)
            iterElement->data[i] = buffer[j];

        iterElement->dataLen = size;
    }
    else
    {
        /* We may have to copy to full and then call
         * addRangingDataElementNew() with remaining
         */
        size = maxSegmentSizeWithHeader;
        iterElement->data = (uint8 *) rasPmemRealloc(iterElement->data, iterElement->dataLen, size);

        for (i = iterElement->dataLen ; i< size; i++, j++)
            iterElement->data[i] = buffer[j];

        offset = size - iterElement->dataLen;
        iterElement->dataLen = size;
        iterElement->partial = FALSE;

        GATT_RAS_SERVER_INFO("addRangingDataExistingElement Calling addRangingDataElementNew");

        addRangingDataElementNew(
            rasSrver,
            client_index,
            &(rasSrver->data.connectedClients[client_index].rangingDataList),
            &(buffer[offset]),
            (bufLen- offset),
            firstEvent,
            mtu,
            procedureDoneStatus);
    }
}


static bool isAnySegmentPartialEmpty(GRASS_T *rasSrver, uint8 client_index)
{
    bool status = FALSE;
    ras_ranging_data_list *iterElement = 
        rasSrver->data.connectedClients[client_index].rangingDataList;

    while(iterElement != NULL)
    {
        if(iterElement->consumed == TRUE && iterElement->partial == TRUE)
        {
            status = TRUE;
            break;
        }
        iterElement = iterElement->next;
    }
    return status;
}

static uint16 getValueSizeByBitPositionForModeX(uint8 mode,
                                                uint16 bitPosition,
                                                uint8 numAntennaPath)
{
    uint16 size = 0;

    if (mode == RAS_MODE0)
    {
        switch (bitPosition)
        {
            case MODE0_PACKET_QUALITY_BIT:
            case MODE0_PACKET_RSSI_BIT:
            case MODE0_PACKET_ANTENNA_BIT:
                size = RAS_VALUE_SIZE_1OCTET;
                break;
            case MODE0_MEASURED_FREQ_OFFSET_BIT:
                size = RAS_VALUE_SIZE_2OCTET;
               break;

            default:
                size = RAS_VALUE_SIZE_1OCTET;
        }
    }
    else if (mode == RAS_MODE1)
    {
        switch (bitPosition)
        {
            case MODE1_PACKET_QUALITY_BIT:
            case MODE1_PACKET_NADM_BIT:
            case MODE1_PACKET_RSSI_BIT:
            case MODE1_PACKET_ANTENNA_BIT:
                size = RAS_VALUE_SIZE_1OCTET;
                break;
            case MODE1_TOD_TOA_BIT:
                size = RAS_VALUE_SIZE_2OCTET;
                break;
            case MODE1_PACKET_PCT1_BIT:
            case MODE1_PACKET_PCT2_BIT:
                size = RAS_VALUE_SIZE_4OCTET;
                break;

            default:
                size = RAS_VALUE_SIZE_1OCTET;
        }
    }
    else if (mode == RAS_MODE2)
    {
        switch (bitPosition)
        {
            case MODE2_ANTENNA_PERMUTATION_INDEX_BIT:
                size = RAS_VALUE_SIZE_1OCTET;
                break;

            case MODE2_TONE_PCT_BIT:
            case MODE2_TONE_QUALITY_INDICATOR_BIT:
            case MODE2_TONE_ANTENNA_PATH_1_BIT:
            case MODE2_TONE_ANTENNA_PATH_2_BIT:
            case MODE2_TONE_ANTENNA_PATH_3_BIT:
            case MODE2_TONE_ANTENNA_PATH_4_BIT:
                /* We will return the size of full Tone_PCT[k] +
                 * Tone_Quality_Indicator[k]
                 */
                size = ((numAntennaPath + 1)*3) + ((numAntennaPath + 1)*1);
                break;

            default:
                size = RAS_VALUE_SIZE_1OCTET;
        }
    }
    else
    {
        switch (bitPosition)
        {
            case MODE3_PACKET_QUALITY_BIT:
            case MODE3_PACKET_NADM_BIT:
            case MODE3_PACKET_RSSI_BIT:
            case MODE3_PACKET_ANTENNA_BIT:
            case MODE3_ANTENNA_PERMUTATION_INDEX_BIT:
                size = RAS_VALUE_SIZE_1OCTET;
                break;

            case MODE3_TOD_TOA_BIT:
                size = RAS_VALUE_SIZE_2OCTET;
                break;

            case MODE3_PACKET_PCT1_BIT:
            case MODE3_PACKET_PCT2_BIT:
                size = RAS_VALUE_SIZE_4OCTET;
                break;

            case MODE3_TONE_PCT_BIT:
            case MODE3_TONE_QUALITY_INDICATOR_BIT:
            case MODE3_TONE_ANTENNA_PATH_1_BIT:
            case MODE3_TONE_ANTENNA_PATH_2_BIT:
            case MODE3_TONE_ANTENNA_PATH_3_BIT:
            case MODE3_TONE_ANTENNA_PATH_4_BIT:
                /* We will return the size of full Tone_PCT[k] +
                 * Tone_Quality_Indicator[k]
                 */
                size = ((numAntennaPath + 1)*3) + ((numAntennaPath + 1)*1);
                break;

            default:
                size = RAS_VALUE_SIZE_1OCTET;
        }
    }

    return size;
}

static uint8 getAntennaPositionFromPermuatationArray(uint8 antennaPathPermutationIndex,
                                                     uint8 numAntennaPath,
                                                     uint8 position)
{
    uint8 antennaPathPosition;

    if(position > numAntennaPath)
        return 0;

    if(numAntennaPath == ANTENNA_PATH_AP2  && antennaPathPermutationIndex > ANTENNA_PATH_PERMUTATION_INDEX_AP2_MAX_VALUE)
        return 0;

    if(numAntennaPath == ANTENNA_PATH_AP3  && antennaPathPermutationIndex > ANTENNA_PATH_PERMUTATION_INDEX_AP3_MAX_VALUE)
        return 0;

    if(numAntennaPath == ANTENNA_PATH_AP4  && antennaPathPermutationIndex > ANTENNA_PATH_PERMUTATION_INDEX_AP4_MAX_VALUE)
        return 0;

    switch (numAntennaPath)
    {
        case ANTENNA_PATH_AP2:
            antennaPathPosition = antennaPathNum2[antennaPathPermutationIndex][position];
            break;

        case ANTENNA_PATH_AP3:
            antennaPathPosition = antennaPathNum3[antennaPathPermutationIndex][position];
            break;

        case ANTENNA_PATH_AP4:
            antennaPathPosition = antennaPathNum4[antennaPathPermutationIndex][position];
            break;

        default:
            antennaPathPosition = 0;
            break;
    }

    return antennaPathPosition;
}

static void copyFilteredData(uint8* rangingBuffer,
                             uint16* rangingBufferCount,
                             uint8* buffer,
                             uint16 bufferCount,
                             uint8 mode,
                             uint16 filterMask,
                             uint8 antennaPathPermutationIndex,
                             uint8 numAntennaPath)
{
    uint16 a1 =0,a2= 0,a3 =0 ,a4 = 0;
    uint8 i;
    uint8 k =0;
    uint8 antenna[4];
    uint8 num;
    uint16 tonePct;
    uint16 toneQualityIndicator;

    /*Example: 02 47 15 0B 2E 0F FD 03 28 3F FD 03 28 6F FD 03 24 AF FD 03 2A 7F FD 23 */

    /* Identify which all antenna path filtered bits are set */

    if (mode == RAS_MODE2)
    {
        tonePct = (filterMask & (1 << MODE2_TONE_PCT_BIT));
        toneQualityIndicator = (filterMask & (1 << MODE2_TONE_QUALITY_INDICATOR_BIT));

        GATT_RAS_SERVER_INFO("copyFilteredData tonePct %x --->", tonePct);
        GATT_RAS_SERVER_INFO("copyFilteredData toneQualityIndicator %x --->", toneQualityIndicator);

        if( tonePct > 0)
        {
            a1 = (filterMask & (1 << MODE2_TONE_ANTENNA_PATH_1_BIT));
            a2 = (filterMask & (1 << MODE2_TONE_ANTENNA_PATH_2_BIT));
            a3 = (filterMask & (1 << MODE2_TONE_ANTENNA_PATH_3_BIT));
            a4 = (filterMask & (1 << MODE2_TONE_ANTENNA_PATH_4_BIT));
        }
    }
    else
    {
        /* Mode 3 */
        tonePct = (filterMask & (1 << MODE3_TONE_PCT_BIT));
        toneQualityIndicator = (filterMask & (1 << MODE3_TONE_QUALITY_INDICATOR_BIT));

        if( tonePct > 0)
        {
            a1 = (filterMask & (1 << MODE3_TONE_ANTENNA_PATH_1_BIT));
            a2 = (filterMask & (1 << MODE3_TONE_ANTENNA_PATH_2_BIT));
            a3 = (filterMask & (1 << MODE3_TONE_ANTENNA_PATH_3_BIT));
            a4 = (filterMask & (1 << MODE3_TONE_ANTENNA_PATH_4_BIT));
        }
    }

    antenna[0] = getAntennaPositionFromPermuatationArray(antennaPathPermutationIndex, numAntennaPath, 0);
    antenna[1] = getAntennaPositionFromPermuatationArray(antennaPathPermutationIndex, numAntennaPath, 1);
    antenna[2] = getAntennaPositionFromPermuatationArray(antennaPathPermutationIndex, numAntennaPath, 2);
    antenna[3] = getAntennaPositionFromPermuatationArray(antennaPathPermutationIndex, numAntennaPath, 3);


    GATT_RAS_SERVER_DEBUG("copyFilteredData antenna[0]: %x", antenna[0]);
    GATT_RAS_SERVER_DEBUG("copyFilteredData antenna[1] %x ", antenna[1]);
    GATT_RAS_SERVER_DEBUG("copyFilteredData antenna[2] %x ", antenna[2]);
    GATT_RAS_SERVER_DEBUG("copyFilteredData antenna[3] %x ", antenna[3]);

    GATT_RAS_SERVER_DEBUG("copyFilteredData a1:%x, a2:%x, a3:%x, a4:%x", a1,a2, a3, a4);

    for(i = 0; i < numAntennaPath; i++)
    {
        num = antenna[k];
        if(tonePct > 0)
        {
            if((num == ANTENNA_PATH_AP1 && a1 > 0) ||
               (num == ANTENNA_PATH_AP2 && a2 > 0) ||
               (num == ANTENNA_PATH_AP3 && a3 > 0) ||
               (num == ANTENNA_PATH_AP4 && a4 > 0))
            {
                GATT_RAS_SERVER_INFO("copyFilteredData ANTENNA_PATH_AP%x ", num);
                rangingBuffer[*rangingBufferCount] = buffer[bufferCount];
                rangingBuffer[*rangingBufferCount+ 1] = buffer[bufferCount + 1];
                rangingBuffer[*rangingBufferCount+ 2] = buffer[bufferCount + 2];
                *rangingBufferCount = *rangingBufferCount + 3;
            }
        }
        bufferCount = bufferCount + 3;

        if (toneQualityIndicator > 0)
        {
            rangingBuffer[*rangingBufferCount] = buffer[bufferCount];
            *rangingBufferCount = *rangingBufferCount + 1;
        }

        bufferCount = bufferCount + 1;
        k++;
    }

    num = buffer[bufferCount+3];
    GATT_RAS_SERVER_INFO("copyFilteredData extenstion slot check %x ", num);

    /* Check if extension slot is present and it contains tone and
     * tone has any of filter set antenna value
     */
     if (((buffer[bufferCount+3] & TONE_PRESENT_IN_EXTENSION_SLOT) == TONE_PRESENT_IN_EXTENSION_SLOT) ||
         ((buffer[bufferCount+3] & TONE_ABSENT_IN_EXTENSION_SLOT) == TONE_ABSENT_IN_EXTENSION_SLOT))
     {
         num = antenna[numAntennaPath-1];
         if((num == ANTENNA_PATH_AP1 && a1 > 0) ||
            (num == ANTENNA_PATH_AP2 && a2 > 0) ||
            (num == ANTENNA_PATH_AP3 && a3 > 0) ||
            (num == ANTENNA_PATH_AP4 && a4 > 0))
         {
             GATT_RAS_SERVER_INFO("copyFilteredData extenstion slot ANTENNA_PATH_AP%x ", num);
             rangingBuffer[*rangingBufferCount] = buffer[bufferCount];
             rangingBuffer[*rangingBufferCount+ 1] = buffer[bufferCount + 1];
             rangingBuffer[*rangingBufferCount+ 2] = buffer[bufferCount + 2];
             *rangingBufferCount = *rangingBufferCount + 3;
         }
     }

     if (toneQualityIndicator > 0)
     {
        rangingBuffer[*rangingBufferCount] = buffer[bufferCount+3];
        *rangingBufferCount = *rangingBufferCount + 1;
     }
}

static void copyStepData(uint8* rangingBuffer,
                         uint16 *count,
                         uint8* buffer,
                         uint8 *stepOffset,
                         uint8 mode,
                         uint8 stepDataPacketSize,
                         uint16 filterMask,
                         uint8 numAntennaPath)
{
    uint16 size =0;
    uint16 j = *stepOffset + 3;
    uint16 tempCount = *count;
    uint16 bitPosition = 0;
    uint8  antennaPathPermutationIndex;
    uint16 valueSize;
    rangingBuffer[tempCount++] = ((mode) | SUCCESS);

    if (filterMask == DEFAULT_FILTER_VALUE_OFF)
    {
        GATT_RAS_SERVER_INFO("DEFAULT_FILTER_VALUE_OFF");
        while(size < stepDataPacketSize - 3)
        {  
            rangingBuffer[tempCount++] = buffer[j++];
            size++;
        }
        *stepOffset+= stepDataPacketSize;
        *count = tempCount;
    }
    else
    {
        GATT_RAS_SERVER_INFO("copyStepData filterMask %x --->", filterMask);

        /* Filter is applied, we need to knock off those octets which are not
        * asked by remote client for each mode
        */

        if (mode == RAS_MODE0)
        {
            while(size < stepDataPacketSize - 3)
            {
                valueSize = getValueSizeByBitPositionForModeX(mode, bitPosition, numAntennaPath);
                if ((filterMask & (1 << bitPosition)) > 0)
                {
                    GATT_RAS_SERVER_DEBUG("bitPosition %x --->", bitPosition);
                    if (bitPosition == MODE0_MEASURED_FREQ_OFFSET_BIT)
                    {
                        rangingBuffer[tempCount++] = buffer[j];
                        rangingBuffer[tempCount++] = buffer[j+1];
                    }
                    else
                    {
                        /* All other Mode 0 data is 1 octet each */
                        rangingBuffer[tempCount++] = buffer[j];
                    }
                }
                size = size + valueSize;
                j = j + valueSize;
                bitPosition++;
            }
        }
        else if (mode == RAS_MODE1)
        {
            while(size < stepDataPacketSize - 3)
            {
                valueSize = getValueSizeByBitPositionForModeX(mode, bitPosition, numAntennaPath);
                if ((filterMask & (1 << bitPosition)) > 0)
                {
                    GATT_RAS_SERVER_DEBUG("bitPosition %x --->", bitPosition);
                    if (bitPosition == MODE1_TOD_TOA_BIT)
                    {
                        rangingBuffer[tempCount++] = buffer[j];
                        rangingBuffer[tempCount++] = buffer[j+1];
                    }
                    else if (bitPosition == MODE1_PACKET_PCT1_BIT || bitPosition == MODE1_PACKET_PCT1_BIT)
                    {
                        rangingBuffer[tempCount++] = buffer[j];
                        rangingBuffer[tempCount++] = buffer[j+1];
                        rangingBuffer[tempCount++] = buffer[j+2];
                        rangingBuffer[tempCount++] = buffer[j+3];
                    }
                    else
                    {
                        /* All other Mode 1 data is 1 octet each */
                        rangingBuffer[tempCount++] = buffer[j];
                    }
                }
                size = size + valueSize;
                j = j + valueSize;
                bitPosition++;
            }
        }
        else if (mode == RAS_MODE2)
        {
            antennaPathPermutationIndex = buffer[j + MODE2_ANTENNA_PERMUTATION_OFFSET];
            GATT_RAS_SERVER_INFO("copyStepData Mode 2 antennaPathPermutationIndex %x --->", antennaPathPermutationIndex);

            while(size < stepDataPacketSize - 3)
            {
                valueSize = getValueSizeByBitPositionForModeX(mode, bitPosition, numAntennaPath);

                if ((filterMask & (1 << bitPosition)) > 0)
                {
                    GATT_RAS_SERVER_DEBUG("bitPosition %x --->", bitPosition);
                    if (bitPosition == MODE2_ANTENNA_PERMUTATION_INDEX_BIT)
                    {
                        antennaPathPermutationIndex = buffer[j];
                        GATT_RAS_SERVER_DEBUG("copyStepData antennaPathPermutationIndex %x --->", antennaPathPermutationIndex);
                        rangingBuffer[tempCount++] = buffer[j];
                    }
                    else if (bitPosition >= MODE2_TONE_PCT_BIT || 
                             bitPosition <= MODE2_TONE_ANTENNA_PATH_4_BIT)
                    {
                        GATT_RAS_SERVER_DEBUG("copyStepData numAntennaPath %x --->", numAntennaPath);

                        if (numAntennaPath == 0)
                        {
                            if(bitPosition == MODE2_TONE_PCT_BIT)
                            {
                                rangingBuffer[tempCount++] = buffer[j];
                                rangingBuffer[tempCount++] = buffer[j+1];
                                rangingBuffer[tempCount++] = buffer[j+2];
                            }

                            if((filterMask & (1 << MODE2_TONE_QUALITY_INDICATOR_BIT)) > 0)
                            {
                                rangingBuffer[tempCount++] = buffer[j+3];
                            }
                        }
                        else
                        {
                            /* We have now antennaPathPermutationIndex and numAntennaPath info
                             * we need to refer now lookup table at what position "filtered"
                             * antenna path_x values are present
                             */
                            GATT_RAS_SERVER_DEBUG("copyStepData Mode 3 antennaPath %x tempCount =%x--->", antennaPathPermutationIndex, tempCount);
                            copyFilteredData(rangingBuffer, &tempCount, buffer, j, RAS_MODE2, filterMask,
                                             antennaPathPermutationIndex, numAntennaPath);
                        }
                        bitPosition = MODE2_TONE_ANTENNA_PATH_4_BIT;
                    }
                    else
                    {
                         /* All other Mode 2 data is 1 octet each */
                         rangingBuffer[tempCount++] = buffer[j];
                    }
                }
                size = size + valueSize;
                j = j + valueSize;
                bitPosition++;
            }
        }

        else if (mode == RAS_MODE3)
         {
            antennaPathPermutationIndex = buffer[j + MODE3_ANTENNA_PERMUTATION_OFFSET];
            GATT_RAS_SERVER_INFO("copyStepData antennaPathPermutationIndex %x --->", antennaPathPermutationIndex);

             while(size < stepDataPacketSize - 3)
             {
                 valueSize = getValueSizeByBitPositionForModeX(mode, bitPosition, numAntennaPath);

                 if ((filterMask & (1 << bitPosition)) > 0)
                 {
                     GATT_RAS_SERVER_DEBUG("bitPosition %x --->", bitPosition);
                     if (bitPosition == MODE3_TOD_TOA_BIT)
                     {
                         rangingBuffer[tempCount++] = buffer[j];
                         rangingBuffer[tempCount++] = buffer[j+1];
                     }
                     else if (bitPosition == MODE3_PACKET_PCT1_BIT || bitPosition == MODE3_PACKET_PCT2_BIT)
                     {
                         rangingBuffer[tempCount++] = buffer[j];
                         rangingBuffer[tempCount++] = buffer[j+1];
                         rangingBuffer[tempCount++] = buffer[j+2];
                         rangingBuffer[tempCount++] = buffer[j+3];
                     }
                     else if (bitPosition == MODE3_ANTENNA_PERMUTATION_INDEX_BIT)
                     {
                         antennaPathPermutationIndex = buffer[j];
                         GATT_RAS_SERVER_DEBUG("copyStepData antennaPathPermutationIndex %x --->", antennaPathPermutationIndex);
                         rangingBuffer[tempCount++] = buffer[j];
                     }
                     else if (bitPosition >= MODE3_TONE_PCT_BIT || 
                              bitPosition <= MODE3_TONE_ANTENNA_PATH_4_BIT)
                     {
                        if (numAntennaPath == 0)
                        {
                            if(bitPosition == MODE3_TONE_PCT_BIT)
                            {
                                rangingBuffer[tempCount++] = buffer[j];
                                rangingBuffer[tempCount++] = buffer[j+1];
                                rangingBuffer[tempCount++] = buffer[j+2];
                            }

                            if((filterMask & (1 << MODE3_TONE_QUALITY_INDICATOR_BIT)) > 0)
                            {
                                rangingBuffer[tempCount++] = buffer[j+3];
                            }
                        }
                        else
                        {
                          /* We have now antennaPathPermutationIndex and numAntennaPath info
                           * we need to refer now lookup table at what position "filtered"
                           * antenna path_x values are present
                           */
                          copyFilteredData(rangingBuffer, &tempCount, buffer, j, RAS_MODE3, filterMask,
                                           antennaPathPermutationIndex, numAntennaPath);
                        }
                        bitPosition = MODE3_TONE_ANTENNA_PATH_4_BIT;
                     }
                     else
                     {
                          /* All other Mode 3 data is 1 octet each */
                          rangingBuffer[tempCount++] = buffer[j];
                     }
                 }
                 size = size + valueSize;
                 j = j + valueSize;
                 bitPosition++;
             }
         }

        *stepOffset+= stepDataPacketSize;
        *count = tempCount;
    }
}

static uint8* getSegmentData(GRASS_T *rasSrver,uint8* size, uint8 client_index)
{
    uint8 *data = NULL;
    bool rangingDataRequested = 
            rasSrver->data.connectedClients[client_index].rangingDataRequested;

    ras_ranging_data_list *list =
            rasSrver->data.connectedClients[client_index].rangingDataList;

    if(rangingDataRequested)
    {
        while(list != NULL)
        {
            if (list->transmitted == FALSE &&
                !list->partial)
            {
                data = (uint8*) list->data;
                *size = (uint8) list->dataLen;
                list->transmitted = TRUE;
                /* displayRasSegmentData(data, *size, (*size < 120 ? TRUE: FALSE)); */
                GATT_RAS_SERVER_INFO("Segment Size: %x ", *size);
                break;
            }
            list = list->next;
        }
    }

    return data;
}

static void setLastSegment2Transmit(GRASS_T *rasSrver, uint8 client_index)
{
    ras_ranging_data_list *list =
            rasSrver->data.connectedClients[client_index].rangingDataList;

    while(list != NULL)
    {
        if (list->transmitted == FALSE &&
            list->partial)
        {
           list->partial = FALSE;
           list->data[0] |= LAST_SEGMENT_TRUE;
           break;
        }
        list = list->next;
    }

    GATT_RAS_SERVER_INFO("setSegment2Transmit final done");
}

static void rasServerUpdateCsProcedureInfo(uint8 *rangingBuffer,
                                           uint8 subeventDoneStatusOffset,
                                           uint8 numOfStepsOffset,
                                           uint8 procedureDoneStatus,
                                           uint8 numOfStepsReported,
                                           uint8 subeventDoneStatus,
                                           uint8 subeventAbortStatus)
{
    /* Ranging Done Status */
    rangingBuffer[subeventDoneStatusOffset] = (procedureDoneStatus & 0x0F);

    /* Subevent Done Offset */
    rangingBuffer[subeventDoneStatusOffset] =
         (rangingBuffer[subeventDoneStatusOffset] | ((subeventDoneStatus & 0x0F) << 4));

    /* Increment the offset for abort status */
    subeventDoneStatusOffset++;
    /* Ranging Abort Reason & Subevent Abort Reason */
    rangingBuffer[subeventDoneStatusOffset] = subeventAbortStatus;

    GATT_RAS_SERVER_INFO("Num of steps before: %d , reported =%d", rangingBuffer[numOfStepsOffset], numOfStepsReported);

    rangingBuffer[numOfStepsOffset] =  rangingBuffer[numOfStepsOffset] + numOfStepsReported;

    GATT_RAS_SERVER_DEBUG("Num of steps after: %d ", rangingBuffer[numOfStepsOffset]);
}

void rasServerSetAllSegmentToTransmit(GRASS_T *rasServer, uint8 client_index)
{
    ras_ranging_data_list *list = 
        rasServer->data.connectedClients[client_index].rangingDataList;

    while(list != NULL)
    {
        list->transmitted = FALSE;
        list = list->next;
    }

    GATT_RAS_SERVER_INFO("rasServerSetAllSegmentToTransmit done.. Ready to transmit again!");

}

void rasServerSendRangingData(GRASS_T *rasServer,
                              ConnectionIdType cid,
                              uint16 rangingCounter,
                              uint16 handle)
{
    uint8* data;
    uint8 i, size;
    uint8 client_index = rasServerGetCidIndex(rasServer,cid);
    CSR_UNUSED(rangingCounter);

    GATT_RAS_SERVER_INFO("Num of Segments: %d ", rasServer->data.connectedClients[client_index].numOfSegments);

    for (i = 1; i<= rasServer->data.connectedClients[client_index].numOfSegments ; i++)
    {
        /* Currently supports  1 CS Procedure  */
        data = getSegmentData(rasServer, &size, client_index);

        if(data != NULL)
        {
            GATT_RAS_SERVER_INFO("Sending Segment size: %d, index %d", size, i);

            rasServerSendRasData(rasServer,
                                cid,
                                handle,
                                size,
                                data);
        }
        if(rasServer->data.connectedClients[client_index].triggerCsProcedureComplete == TRUE 
           && i == rasServer->data.connectedClients[client_index].numOfSegments)
        {
            rasServer->data.connectedClients[client_index].allSegmentsSent = TRUE;
        }
    }

    if (rasServer->data.connectedClients[client_index].triggerCsProcedureComplete &&
         rasServer->data.connectedClients[client_index].rangingDataRequested)
    {
        rasServer->data.connectedClients[client_index].triggerCsProcedureComplete = FALSE;

        /* If RealTime Ranging Data enabled, no need to inform Application */
        if(rasServer->data.connectedClients[client_index].clientCfg.realTimeDataClientCfg & GATT_RAS_CLIENT_CONFIG_MASK)
        {
            GATT_RAS_SERVER_DEBUG("RealTime Ranging Data Send. Flush all segments!");
            rasServer->data.connectedClients[client_index].procedureActive = FALSE;

            /* Free the list */
            rasServerFreeRangingDataList(rasServer, client_index);

            /* Reset number of segments */
            rasServer->data.connectedClients[client_index].numOfSegments =0;

            return;
        }

        /* Notify the Application */
        rasServerNotifyToApp(rasServer,
                             cid,
                             GATT_RAS_SERVER_OP_COMPLETE_RANGING_DATA,
                             rasServer->data.connectedClients[client_index].curRangingCounter,
                             0,
                             0);
    }
}

static uint16 rasServerConstructRangingHeader(GRASS_T *rasSrver,
                                              CmLeCsSubeventResultInd *ind,
                                              uint8 *rangingBuffer,
                                              uint16 offset,
                                              uint8 selectedTxPower)
{
    uint16 count = offset;

    /* We need to construct ranging header in the rangingDataBuffer */
    uint8 configId = (ind->configId & 0x0F); /* Need only 4 bits as config is between 0-3) */

    CSR_UNUSED(rasSrver);

    rangingBuffer[count++] = (ind->procedureCounter & 0xFF);
    rangingBuffer[count++] = ((configId << 4) | ((ind->procedureCounter >> 8) & 0x0F));
    rangingBuffer[count++] = selectedTxPower;

    if(ind->numAntennaPaths == 1)
    {
        rangingBuffer[count] = ANTENNA_PATH_1;
    }
    else if(ind->numAntennaPaths == 2)
    {
        rangingBuffer[count] = ANTENNA_PATH_1 | ANTENNA_PATH_2;
    }
    else if(ind->numAntennaPaths == 3)
    {
        rangingBuffer[count] = ANTENNA_PATH_1 | ANTENNA_PATH_2 | ANTENNA_PATH_3;
    }
    else if(ind->numAntennaPaths == 4)
    {
        rangingBuffer[count] = ANTENNA_PATH_1 | ANTENNA_PATH_2 | ANTENNA_PATH_3 | ANTENNA_PATH_4;
    }

    /* IQ is by default 0x0 in BTSS and Phase/IQ format feature
     * not shown enabled in Profile 
     * 2 bits will be 0 by default.
     */
     rangingBuffer[count] |= (0x00 << 6);
     count++;
     return count;
}

static uint16 rasServerConstructSubeventHeader(GRASS_T *rasSrver,
                                               CmLeCsSubeventResultInd * cfm,
                                               uint8 *rangingBuffer,
                                               uint16 offset,
                                               uint8 client_index)
{
    uint16 count = offset;

    /* Construct Subevent Header */
    rangingBuffer[count++] = (cfm->startAclConnEventCounter & 0xFF);
    rangingBuffer[count++] = ((cfm->startAclConnEventCounter >> 8) & 0xFF);

    rangingBuffer[count++] = (cfm->frequencyCompensation & 0xFF);
    rangingBuffer[count++] = ((cfm->frequencyCompensation >> 8) & 0xFF);

    /* Ranging Done Status */
    rangingBuffer[count] = (cfm->procedureDoneStatus & 0x0F);
    /* Subevent Done Offset */
    rangingBuffer[count] = (rangingBuffer[count] | ((cfm->subeventDoneStatus & 0x0F) << 4));
    rasSrver->data.connectedClients[client_index].subeventDoneStatusOffset = (uint8) count;
    count++;

    /* Ranging Abort Reason & Subevent Abort Reason */
    rangingBuffer[count++] = cfm->abortReason;

    /* reference power level */
    rangingBuffer[count++] = cfm->referencePowerLevel;

    /* Num Steps Reported */
    rangingBuffer[count] = cfm->numStepsReported;
    GATT_RAS_SERVER_INFO("Num of steps first: %d ", rangingBuffer[count]);

    rasSrver->data.connectedClients[client_index].numOfStepsOffset = (uint8) count;

    GATT_RAS_SERVER_INFO("numOfStepsOffset =%d", rasSrver->data.connectedClients[client_index].numOfStepsOffset);

    count++;

    return count;
}

static uint16 rasServerConstructSubeventStepsData(GRASS_T *rasSrver,
                                                  uint8 client_index,
                                                  uint8 *rangingBuffer,
                                                  uint16 offset,
                                                  uint8 numStepsReported,
                                                  uint8 *steps,
                                                  uint8 numAntennaPath)
{
    uint8 i,stepOffset = 0;
    uint16 count = offset;
    uint8 stepDataPacketSize = 0;
    uint8 mode;
    GATT_RAS_SERVER_INFO("convertToRangingData  numSteps %x --->", numStepsReported);

    /* Construct Subevent Data */

    /* We need to remove redundant data items like step datalen and
     * step channel
     * Mode information to be converted accordingly as per spec
     */
    for (i = 0; i < numStepsReported ; i ++)
    {
        mode = steps[stepOffset];

        /* length field is at offset 2 in step data. 
         * step data packet contains "mode", channel and length field also, add 3
         */
        stepDataPacketSize = steps[stepOffset + 2] + 3;

        copyStepData(rangingBuffer, &count,
            steps, &stepOffset,
            mode, stepDataPacketSize,
            rasSrver->data.connectedClients[client_index].filterMask[mode],
            numAntennaPath);
    }

    /* displayRasSegmentData(rangingBuffer, count, FALSE); */

    GATT_RAS_SERVER_INFO("convertToRangingData Count %d StepOffset %d --->", count, stepOffset);
    return count;
}

static void rasServerConvertToSegments(GRASS_T *rasSrver,
                                       uint16 client_index,
                                       uint8 *buffer,
                                       uint16 bufLen,
                                       bool firstEvent,
                                       uint16 mtu,
                                       uint8 procedureDoneStatus)
{
    GATT_RAS_SERVER_INFO("rasServerConvertToSegments --->");

    /* Check if there is any segment already which is partial full
     * If no, then we are good to go to create fresh segments and we can call
     * addRangingDataElementNewTest() to add new elements.
     * If there is a segment which is partial full, then we need to 
     * call addRangingDataExistingElementTest()
     */
    if(isAnySegmentPartialEmpty(rasSrver, (uint8) client_index))
    {
        addRangingDataExistingElement(
           rasSrver,
           client_index,
           &(rasSrver->data.connectedClients[client_index].rangingDataList),
           buffer,
           bufLen,
           FALSE,
           mtu,
           procedureDoneStatus);
    }
    else
    {
        addRangingDataElementNew(
           rasSrver,
           client_index,
           &(rasSrver->data.connectedClients[client_index].rangingDataList),
           buffer,
           bufLen,
           firstEvent,
           mtu,
           procedureDoneStatus);
    }
}

static void rasServerCsSubeventResultInd(GRASS_T* rasSrver,
                                         CmLeCsSubeventResultInd * ind)
{
    bool firstEvent = FALSE;
    ConnectionIdType cid = CsrBtGattClientUtilFindConnIdByAddr(&ind->addrt);
    uint8 client_index;
    uint8 selectedTxpower;
    uint16 handle = HANDLE_ONDEMAND_RANGING_DATA;

    client_index = rasServerGetCidIndex(rasSrver, cid);

    if(client_index == GATT_RAS_SERVER_INVALID_CID_INDEX)
    {
        GATT_RAS_SERVER_INFO("rasServerCsSubeventResultInd No device connected");
        return;
    }

    GATT_RAS_SERVER_INFO("rasServerCsSubeventResultInd Result Code:0x%x", ind->resultCode);

    GATT_RAS_SERVER_INFO("%x %x %x %x %x %x %x %x %x %x", ind->configId, ind->startAclConnEventCounter,
        ind->procedureCounter, ind->frequencyCompensation, ind->referencePowerLevel,
        ind->procedureDoneStatus, ind->subeventDoneStatus, ind->abortReason,
        ind->numAntennaPaths, ind->numStepsReported);

    /* Display HCI event */
    /* displayHciStepData(ind->steps, ind->stepsDataLen); */

    /* We have received CS Subevent data */
    if(rasSrver->data.connectedClients[client_index].procedureActive == FALSE)
    {
        /* Mark the CS Procedure active now , we have received first subevent result*/
        rasSrver->data.connectedClients[client_index].procedureActive = TRUE;
        /* We are active on said procedure counter, we expect data on this procedure counter */
        rasSrver->data.connectedClients[client_index].curRangingCounter= (0xFFF & ind->procedureCounter);

        if(rasSrver->data.connectedClients[client_index].clientCfg.onDemandDataClientCfg & GATT_RAS_CLIENT_CONFIG_MASK)
        {
            /* We need to send Ranging Data Ready indication to the client */
            /* Notify to application about the Ranging Data available for it
             * to trigger ranging data indication to client
             */
            rasServerNotifyToApp(rasSrver,
                                 cid,
                                 GATT_RAS_SERVER_OP_RANGING_DATA_READY,
                                 ind->procedureCounter,
                                 0,
                                 0);
        }

        selectedTxpower = rasSrver->data.connectedClients[client_index].selectedTxPower;
        firstEvent = TRUE;

        gRangingDataBufferLen[client_index] = 
                rasServerConstructRangingHeader(rasSrver,
                                                ind,
                                                gRangingDataBuffer[client_index],
                                                RANGING_HEADER_START_OFFSET,
                                                selectedTxpower);
    }
    else
    {
        /* This could be the subsequent CS subevent or first CS subevent of
         * new procedure counter.
         * If it is new procedure counter then we may have to  overwrite
         * previous result stored and send ranging overwrite indication as we don't
         * want to store previous ranging result
         */
         if(rasSrver->data.connectedClients[client_index].curRangingCounter != ind->procedureCounter)
         {
             GATT_RAS_SERVER_INFO("rasServerCsSubeventResultInd we have received new procedure counter:0x%x", ind->procedureCounter);

             rasServerFreeRangingDataList(rasSrver, client_index);

              /* Reset number of segments */
             rasSrver->data.connectedClients[client_index].numOfSegments = 0;

             if(rasSrver->data.connectedClients[client_index].clientCfg.onDemandDataClientCfg & GATT_RAS_CLIENT_CONFIG_MASK)
             {
                 rasSrver->data.connectedClients[client_index].rangingDataRequested = FALSE;

                 rasSrver->data.connectedClients[client_index].lastRangingCounterOverwritten = 
                     rasSrver->data.connectedClients[client_index].curRangingCounter;

                 rasServerNotifyToApp(rasSrver,
                                      cid,
                                      GATT_RAS_SERVER_RANGING_DATA_OVERWRITTEN,
                                      rasSrver->data.connectedClients[client_index].curRangingCounter,
                                      0,
                                      0);

                 /* We need to send Ranging Data Ready indication to the client */
                 /* Notify to application about the new Ranging Data available for it
                  * to trigger ranging data indication to client
                  */
                 rasServerNotifyToApp(rasSrver,
                                      cid,
                                      GATT_RAS_SERVER_OP_RANGING_DATA_READY,
                                      ind->procedureCounter,
                                      0,
                                      0);
             }

             rasSrver->data.connectedClients[client_index].curRangingCounter= (0xFFF & ind->procedureCounter);

             selectedTxpower = rasSrver->data.connectedClients[client_index].selectedTxPower;
             firstEvent = TRUE;
             gRangingDataBufferLen[client_index] =
                    rasServerConstructRangingHeader(rasSrver,
                                                    ind,
                                                    gRangingDataBuffer[client_index],
                                                    RANGING_HEADER_START_OFFSET,
                                                    selectedTxpower);
         }
    }

    gRangingDataBufferLen[client_index] =
                    rasServerConstructSubeventHeader(rasSrver,
                                                     ind,
                                                     gRangingDataBuffer[client_index],
                                                     gRangingDataBufferLen[client_index],
                                                     client_index);

    gRangingDataBufferLen[client_index] = 
                    rasServerConstructSubeventStepsData(rasSrver,
                                                        client_index,
                                                        gRangingDataBuffer[client_index],
                                                        gRangingDataBufferLen[client_index],
                                                        ind->numStepsReported,
                                                        ind->steps,
                                                        ind->numAntennaPaths);

    if(ind->subeventDoneStatus == PARTIAL_RESULTS_MORE_TO_FOLLOW_FOR_CS_SUBEVENT)
    {
        /* Return from here, we will convert into segment when we get all subevent data */
        return;
    }

    rasServerConvertToSegments(rasSrver,
                               client_index,
                               gRangingDataBuffer[client_index],
                               gRangingDataBufferLen[client_index],
                               firstEvent,
                               rasSrver->data.connectedClients[client_index].mtu,
                               ind->procedureDoneStatus);

    /* Reset Ranging Data buffer len for 1 complete subevent. 1 subevent = subevent + subevent conti... */
    gRangingDataBufferLen[client_index] = 0;

    if(ind->procedureDoneStatus == ALL_RESULTS_COMPLETE_FOR_CS_PROCEDURE)
    {
        /* Ensure that final partial segment is also send as procedure is now complete */
        setLastSegment2Transmit(rasSrver, client_index);

        rasSrver->data.connectedClients[client_index].triggerCsProcedureComplete = TRUE;
    }

    if(rasSrver->data.connectedClients[client_index].clientCfg.realTimeDataClientCfg > 0)
    {
         handle = HANDLE_REALTIME_RANGING_DATA;
    }

    /* We need to start sending ranging data to client */
    rasServerSendRangingData(rasSrver, cid, ind->procedureCounter, handle);
}

static void rasServerCsSubeventResultContinueInd(GRASS_T* rasSrver,
                                                 CmLeCsSubeventResultContinueInd * ind)
{
    bool firstEvent = FALSE;
    ConnectionIdType cid = CsrBtGattClientUtilFindConnIdByAddr(&ind->addrt);
    uint8 client_index;
    uint8 subeventDoneStatusOffset;
    uint8 numOfStepsOffset;
    uint16 handle = HANDLE_ONDEMAND_RANGING_DATA;

    client_index = rasServerGetCidIndex(rasSrver, cid);

    if(client_index == GATT_RAS_SERVER_INVALID_CID_INDEX)
     {
         GATT_RAS_SERVER_INFO("rasServerCsSubeventResultInd No device connected");
         return;
     }

    subeventDoneStatusOffset = rasSrver->data.connectedClients[client_index].subeventDoneStatusOffset;
    numOfStepsOffset = rasSrver->data.connectedClients[client_index].numOfStepsOffset;

    GATT_RAS_SERVER_INFO("rasServerCsSubeventResultContinueInd Result Code:0x%x", ind->resultCode);

    GATT_RAS_SERVER_DEBUG("%x %x %x %x %x %x", ind->configId,ind->procedureDoneStatus,
        ind->subeventDoneStatus, ind->abortReason, ind->numAntennaPaths, ind->numStepsReported);

    /* Display HCI event */
    /* displayHciStepData(ind->steps, ind->stepsDataLen); */

    GATT_RAS_SERVER_INFO("gRangingDataBufferLen before :0x%x", gRangingDataBufferLen[client_index]);

    gRangingDataBufferLen[client_index] = 
            rasServerConstructSubeventStepsData(rasSrver,
                                                client_index,
                                                gRangingDataBuffer[client_index],
                                                gRangingDataBufferLen[client_index],
                                                ind->numStepsReported,
                                                ind->steps,
                                                ind->numAntennaPaths);

    GATT_RAS_SERVER_INFO("gRangingDataBufferLen after :0x%x", gRangingDataBufferLen[client_index]);

    /* We need to update num of steps and CS Info (Prc Done Status, Subevent Done Status, steps) */
    rasServerUpdateCsProcedureInfo(gRangingDataBuffer[client_index],
                                   subeventDoneStatusOffset,
                                   numOfStepsOffset,
                                   ind->procedureDoneStatus,
                                   ind->numStepsReported,
                                   ind->subeventDoneStatus,
                                   ind->abortReason);

    if(ind->subeventDoneStatus == PARTIAL_RESULTS_MORE_TO_FOLLOW_FOR_CS_SUBEVENT)
    {
        /* Return from here, we will convert into segment when we get all subevent data */
        return;
    }

    rasServerConvertToSegments(rasSrver,
                               client_index,
                               gRangingDataBuffer[client_index],
                               gRangingDataBufferLen[client_index],
                               firstEvent,
                               rasSrver->data.connectedClients[client_index].mtu,
                               ind->procedureDoneStatus);

    if(ind->subeventDoneStatus == ALL_RESULTS_COMPLETE_FOR_CS_SUBEVENT)
    {
        /* Reset Ranging Data buffer len for 1 complete subevent. 1 subevent = subevent + subevent conti... */
        gRangingDataBufferLen[client_index] = 0;
    }

    if(ind->procedureDoneStatus == ALL_RESULTS_COMPLETE_FOR_CS_PROCEDURE)
    {
       /* Ensure that final partial segment is also send as procedure is now complete */
       setLastSegment2Transmit(rasSrver, client_index);

       rasSrver->data.connectedClients[client_index].triggerCsProcedureComplete = TRUE;
    }

    if(rasSrver->data.connectedClients[client_index].clientCfg.realTimeDataClientCfg > 0)
    {
        handle = HANDLE_REALTIME_RANGING_DATA;
    }

    /* We need to start sending ranging data to client */
    rasServerSendRangingData(rasSrver, cid, rasSrver->data.connectedClients[client_index].curRangingCounter, handle);
}

static void rasServerCsProcedureEnableInd(GRASS_T* rasSrver,
                                          CmLeCsProcedureEnableInd * ind)
{
    GATT_RAS_SERVER_INFO("rasServerCsProcedureEnableInd Result Code:0x%x", ind->resultCode);

    if(ind->resultCode == CSR_BT_RESULT_CODE_SUCCESS)
    {
        MAKE_RAS_MESSAGE(GattRasServerRangingEnableInd);

        message->rasServiceHandle = rasSrver->srvcHndl;
        message->id = GATT_RAS_SERVER_RANGING_ENABLE_IND;
        message->cid = CsrBtGattClientUtilFindConnIdByAddr(&ind->addrt);
        message->csRangingEnable = ind->state;

        if(ind->state != 0)
        {
            uint8 client_index = rasServerGetCidIndex(rasSrver, message->cid);

            GATT_RAS_SERVER_DEBUG("Result Supplier:0x%x, configId:0x%x, toneAntennaConfigSelection:0x%x, selectedTxPower:0x%x",
                ind->resultCode, ind->configId, ind->toneAntennaConfigSelection, ind->selectedTxPower);

            GATT_RAS_SERVER_DEBUG("subeventLen:0x%x, subeventsPerEvent:0x%x, subeventInterval:0x%x, eventInterval:0x%x",
                ind->subeventLen, ind->subeventsPerEvent, ind->subeventInterval, ind->eventInterval);

            GATT_RAS_SERVER_DEBUG("procedureInterval:0x%x, procedureCount:0x%x", ind->procedureInterval, ind->procedureCount);
            rasSrver->data.connectedClients[client_index].selectedTxPower = ind->selectedTxPower;
        }
        RasServerMessageSend(rasSrver->appTask, message);
    }
}

void rasServerFreeRangingDataList(GRASS_T *rasServer, uint8 client_index)
{
    ras_ranging_data_list  *rangingList;

    if(rasServer == NULL)
        return;
    if(rasServer->data.connectedClients[client_index].curRangingCounter)
    {
        GATT_RAS_SERVER_ERROR("rasServerFreeRangingDataList: curRangingCounter: 0x%x,", rasServer->data.connectedClients[client_index].curRangingCounter);
    }


    rangingList = rasServer->data.connectedClients[client_index].rangingDataList;

    GATT_RAS_SERVER_INFO("freeRangingDataList: Freeing the ranging data list...\n");

    while(rangingList != NULL)
    {
        rangingList = rasServer->data.connectedClients[client_index].rangingDataList->next;

        CsrPmemFree(rasServer->data.connectedClients[client_index].rangingDataList->data);
        CsrPmemFree(rasServer->data.connectedClients[client_index].rangingDataList);
        rasServer->data.connectedClients[client_index].rangingDataList = rangingList;
        GATT_RAS_SERVER_DEBUG("freeing...");
    }

    /* Reset the offset stored for this Procedure */
    rasServer->data.connectedClients[client_index].subeventDoneStatusOffset = 0;
    rasServer->data.connectedClients[client_index].numOfStepsOffset = 0;

    /* Reset the rolling counter as the CS procedure is now complete */
    rasServer->data.connectedClients[client_index].rollingCounter = 0;

    /* Reset the globals */
    gRangingDataBufferLen[client_index] = 0;
    CsrMemSet(gRangingDataBuffer[client_index], 0, RANGING_BUFFER_SIZE);
}

void rasServerCmMsgHandler(GRASS_T* rasServer, void *message)
{
    CsrBtCmPrim *primType = (CsrBtCmPrim *) message;

    switch (*primType)
    {
        case CM_LE_CS_PROCEDURE_ENABLE_IND:
            rasServerCsProcedureEnableInd(rasServer,(CmLeCsProcedureEnableInd*)message);
            break;

        case CM_LE_CS_SUBEVENT_RESULT_IND:
            rasServerCsSubeventResultInd(rasServer,(CmLeCsSubeventResultInd *)message);
            break;

        case CM_LE_CS_SUBEVENT_RESULT_CONTINUE_IND:
            rasServerCsSubeventResultContinueInd(rasServer,(CmLeCsSubeventResultContinueInd *)message);
            break;

         default:
             GATT_RAS_SERVER_WARNING("rasServerHandleCMMsg Unhandled CM Primitive: 0x%x", *primType);
             break;
    }

    CsrBtCmFreeUpstreamMessageContents(CSR_BT_CM_PRIM, (void *) message);
}

#endif /* INSTALL_CHANNEL_SOUNDING_SUPPORT */
